#include "ros/ros.h"
#include "ensenso_camera_msgs/RequestDataAction.h"
#include <actionlib/client/simple_action_client.h>
#include "ensenso_camera_msgs/RequestDataGoal.h"
#include "ensenso_camera_msgs/SetParameterAction.h"
#include "std_msgs/Bool.h"
#include "dbg.h"
bool trrigerFlag;
void TrriggerCallback(const std_msgs::BoolConstPtr& msg)
{
    ROS_INFO("TrriggerCallback ...");
    trrigerFlag = true;
}


int main(int argc, char **argv){

    ros::init(argc, argv, "hirop_ensenso_trriger");
    dbg("等待相机启动 ...  ");
    ros::NodeHandle n;

    actionlib::SimpleActionClient<ensenso_camera_msgs::RequestDataAction> * ensensoCameraActionclient = new actionlib::SimpleActionClient<ensenso_camera_msgs::RequestDataAction>("request_data", true); // true -> don't need ros::spin()
    actionlib::SimpleActionClient<ensenso_camera_msgs::SetParameterAction> *EnsensoSetting = new actionlib::SimpleActionClient<ensenso_camera_msgs::SetParameterAction>("set_parameter", true); // true -> don't need ros::spin()

    bool Cameraflag1 = ensensoCameraActionclient->waitForServer(ros::Duration(5));
    bool Cameraflag2 = EnsensoSetting->waitForServer(ros::Duration(5));
    if(!Cameraflag1 || !Cameraflag2){
        dbg("相机启动失败 .. ");
        return -1;
    }
    ensenso_camera_msgs::SetParameterGoal Setgoal;
    Setgoal.parameters.resize(2);
    Setgoal.parameters[0].key = ensenso_camera_msgs::Parameter::MINIMUM_DISPARITY;
    Setgoal.parameters[0].float_value = -120;
    Setgoal.parameters[1].key = ensenso_camera_msgs::Parameter::AUTO_EXPOSURE;
    Setgoal.parameters[1].bool_value = false;

    EnsensoSetting->sendGoal(Setgoal);
    EnsensoSetting->waitForResult(ros::Duration(1.0));

    if (EnsensoSetting->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
        dbg("相机启动成功 ");
    }else{
        printf("Current State: %s\n", ensensoCameraActionclient->getState().toString().c_str());
        return -1;
    }

    ros::Subscriber ImgSub = n.subscribe("/ensenso_trriger", 1, TrriggerCallback);

    ros::Rate loop_rate(0.5);
    bool color_point_Flag =false;
    ensenso_camera_msgs::RequestDataGoal goal;

    while (ros::ok())
    {
        if(trrigerFlag){
            ImgSub.shutdown();
            trrigerFlag = false;

            for(int i = 0; i < 2; i++){
                goal = ensenso_camera_msgs::RequestDataGoal();
                if(color_point_Flag){
                    goal.request_normals = false;
                    goal.request_rectified_images = false;
                    goal.request_depth_image = false;
                    goal.request_point_cloud = false;
                    goal.request_disparity_map = false;
                    goal.request_raw_images = true;
                    color_point_Flag = false;
                }
                else{
                    goal.request_normals = true;
                    goal.request_rectified_images = true;
                    goal.request_depth_image = true;
                    goal.request_point_cloud = true;
                    goal.request_disparity_map = true;
                    goal.request_raw_images = false;
                    color_point_Flag = true;
                }

                ensensoCameraActionclient->sendGoal(goal);
                ensensoCameraActionclient->waitForResult(ros::Duration(1));
                if (ensensoCameraActionclient->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
                      dbg("get image and point  ");
                      return 0;
                }else{
                    std::cout << "get image and point error, Current State: "<<ensensoCameraActionclient->getState().toString()<<std::endl;
                    return -1;
                }
            }

            ImgSub = n.subscribe("/ensenso_trriger", 1, TrriggerCallback);
        }
        loop_rate.sleep();
    }

    delete ensensoCameraActionclient;
}
